#include "PoseGrabber.h"
#include "config.h"
#include <fstream>
#include <cmath>
#include "LogCat.h"
void PoseGrabber::getGroudTruth()
{
	
}

vector<Pose3D>& PoseGrabber::getGroudTruthOffLine(const string GroudTruth_path)
{
	string file_path = GroudTruth_path;
	ifstream FileIn(file_path);
	Eigen::Matrix4d gt_data;
	Eigen::Matrix4d velo2cam, cam2velo;
	Pose3D pose;
	cam2velo << 0, 0, 1, 0,
		-1, 0, 0, 0,
		0, -1, 0, 0.08,
		0, 0, 0, 1;
	velo2cam << 0, -1, 0, 0,
		0, 0, -1, 0,
		1, 0, 0, -0.08,
		0, 0, 0, 1;

	/*
	** if you just want to get the pose_accordingto_velo transfroming from pose_cam
	**	- Camera:   x: right,		   y: down,			z: forward
	**	- Velodyne: velo_x: forward,   velo_y: left,    velo_z: up
	** then read the pose of cam ,and make
	** velo_x = z + 0.27  ;   velo_y = -x ;	velo_z = -y - 0.08;
	** if you need the whole transform , use the code bellow
	*/

	while (FileIn >> gt_data(0, 0) >> gt_data(0, 1) >> gt_data(0, 2) >> gt_data(0, 3)
		>> gt_data(1, 0) >> gt_data(1, 1) >> gt_data(1, 2) >> gt_data(1, 3)
		>> gt_data(2, 0) >> gt_data(2, 1) >> gt_data(2, 2) >> gt_data(2, 3))
	{
		gt_data(3, 0) = 0;
		gt_data(3, 1) = 0;
		gt_data(3, 2) = 0;
		gt_data(3, 3) = 1;
		gt_data = cam2velo * gt_data * velo2cam;

		//generate the yaw pitch roll /rad
		if (gt_data(2, 0) < +1) {
			if (gt_data(2, 0) > -1) {
				pose.pitch = asin(-gt_data(2, 0));
				pose.yaw = atan2(gt_data(1, 0), gt_data(0, 0));
				pose.roll = atan2(gt_data(2, 1), gt_data(2, 2));
			}
			else {
				pose.pitch = M_PI / 2;
				pose.yaw = -atan2(gt_data(1, 2), gt_data(1, 1));
				pose.roll = 0;
			}
		}
		else {
			pose.pitch = -M_PI / 2;
			pose.yaw = atan2(-gt_data(1, 2), gt_data(1, 1));
			pose.roll = 0;
		}
		pose.x = gt_data(0, 3);
		pose.y = gt_data(1, 3);
		pose.z = gt_data(2, 3);
		//save the pose to the ground_truth
		ground_truth.push_back(pose);
		LogCat::h("PoseGrabber","readGroudTruthOffLine",pose);
		//save the transfromMatrix
		transfromMatrix.push_back(gt_data);
	}
	FileIn.close();

	return ground_truth;
}

void PoseGrabber::get(const int index)
{

}



void PoseGrabber::clear()
{
}

PoseGrabber::PoseGrabber()
{
}


PoseGrabber::~PoseGrabber()
{
}
